package mirabilis.org.util;

import java.util.Iterator;

import org.jblas.DoubleMatrix;

import mirabilis.org.IEvaluator;
import mirabilis.org.config.Configuration;
import mirabilis.org.config.LoadProperties;
import mirabilis.org.simplices.Coordinates;
import mirabilis.org.simplices.Simplex;
import mirabilis.org.simplices.Vertex;
import mirabilis.org.termination.Termination;
import mirabilis.samples.cec2014.Cec2014;

public class Line {
	
	
	/**
	 * 
	 * @param init_p is the point, defined in the line, from which we start the optimization.
	 * @param any_p is any point, defined in the line that we want to optimize.
	 * @param initial_step_size Depending on this parameter, the returned Vertex can be at a maximum distance of @param init_p of 2 times the initial_step_size 
	 * in either directions. This means that we cover 4 times the
	 * @param steps is the minimum number of steps that we perform in the search when every evaluation is successful (improves the search). Opposite, the 
	 * maximum number of evaluations is 2*steps if every step is unsuccessful (the search does not improve).
	 * @param evaluator is a IEvaluator with the fitness function F.
	 * @return A Vertex (let's call it P) with coordinates in the line defined by (init_p,any_p) with the following property:
	 * Assuming the line is unimodal in F, the distance of this point P to the optimal value in the interval [init_p-2*initial_step_size , init_p+2*initial_step_size] is of
	 * max. 1/(2^steps+1) * 4 * initial_step_size 
	 */
	public static Vertex shootcorrection(Coordinates init_p, double[] stabilized_grad, double initial_step_size, int steps, double[] target, double[] grad, IEvaluator evaluator,Simplex simplex){
		
	
		//------------------------------------------------------------------
		// The second step is to optimize a line that is inscribed in the box (i.e. because none of the previous conditions has return something.)
		// That means that we have a line defined within the box.
		//------------------------------------------------------------------		
	
		double distance = initial_step_size;
		
		//initially the best so far point in the line is the init_p point
		// unless init_p is itself outside the bounds. If that is the case, we return the closest intersecting point
		double[] aux = Gradients.getIntersectionOfaVectorWithHyperplane(grad, init_p.getCoordinatesAsDouble(), target);
		Vertex bestsofar = new Vertex(new Coordinates(aux));
		//bestsofar.setFitness(evaluator.evaluate(aux));
		bestsofar.setFitness(Bounds.fitnessWithWallAtTheBounds(aux, simplex));
		
		// Some initializations
		double[] p1 = aux;
		double[] p2 = new double[aux.length];

		
		//For the given No. of steps
		for(int i=0;i<steps;i++){
			
			//ref_hyperplane is an hyperplane that:
			// is orthogonal to stabilized_grad
			// contains p1
			double[] ref_hyperplane = getHyperplane(p1,stabilized_grad);
			
			// p2 is a point in the line
			for(int j=0;j<stabilized_grad.length;j++){
				p2[j] = stabilized_grad[j]+ p1[j];
			}
			
			//we find the two parallel hyperplanes to the ref_hyperplane at the given distance "distance"
			double[][] targetsinline = getTargets(distance, p2, p1, ref_hyperplane);
			
			
			//we calculate a new point in the outer target
			double[] p3 = Gradients.getIntersectionOfaVectorWithHyperplane(stabilized_grad, p2, targetsinline[1]);
			
			double[] newpoint = Gradients.getIntersectionOfaVectorWithHyperplane(grad, p3, target);
			Vertex x = new Vertex(new Coordinates(newpoint));
			x.setFitness(Bounds.fitnessWithWallAtTheBounds(newpoint, simplex));
			
			if(x.better(bestsofar)){ // if p3 is better than the best so far
				bestsofar = x;
				p2 = p1.clone();
				p1 = p3.clone();
			}else{ // otherwise we look for a new point in the inner target
				p3 = Gradients.getIntersectionOfaVectorWithHyperplane(stabilized_grad, p2, targetsinline[0]);
				newpoint = Gradients.getIntersectionOfaVectorWithHyperplane(grad, p3, target);
				x = new Vertex(new Coordinates(newpoint));
				x.setFitness(Bounds.fitnessWithWallAtTheBounds(newpoint, simplex));
				
				if(x.better(bestsofar)){ 
					bestsofar = x;
					p2 = p1.clone();
					p1 = p3.clone();
				}else{
					p2 = p3.clone();
				}
			}
					
			distance /= 2.0;
			
		}
		
		return bestsofar;
	}

	/**
	 * 
	 * @param init_p is the point, defined in the line, from which we start the optimization.
	 * @param any_p is any point, defined in the line that we want to optimize.
	 * @param initial_step_size Depending on this parameter, the returned Vertex can be at a maximum distance of @param init_p of 2 times the initial_step_size 
	 * in either directions. This means that we cover 4 times the
	 * @param steps is the minimum number of steps that we perform in the search when every evaluation is successful (improves the search). Opposite, the 
	 * maximum number of evaluations is 2*steps if every step is unsuccessful (the search does not improve).
	 * @param evaluator is a IEvaluator with the fitness function F.
	 * @return A Vertex (let's call it P) with coordinates in the line defined by (init_p,any_p) with the following property:
	 * Assuming the line is unimodal in F, the distance of this point P to the optimal value in the interval [init_p-2*initial_step_size , init_p+2*initial_step_size] is of
	 * max. 1/(2^steps+1) * 4 * initial_step_size 
	 */
	public static Vertex optimizeline(Coordinates init_p, Coordinates any_p, double initial_step_size, int steps, IEvaluator evaluator){
		
		//------------------------------------------------------------------
		// The first step is to compute the bounds of the line (intersecting points with the "box")
		//------------------------------------------------------------------
		double[][] _boundsOfLine;
		
		try {
			_boundsOfLine = Bounds.findBoundsOfLine(init_p.getCoordinatesAsDouble(), any_p.getCoordinatesAsDouble());
		} catch (Exception e) {// init_p and any_p are the same point or Matrix is singular (no line can be defined)
			//e.printStackTrace();
			if(Bounds.isCoordinateOutsideTheBounds(init_p.getCoordinatesAsDouble())){ //If the point is outside the bounds 
				return null;
			}else{//If is inside the bounds we return the point
				Vertex x = new Vertex(init_p);
				x.setFitness(evaluator.evaluate(init_p.getCoordinatesAsDouble()));
				Termination.increment_evaluation();// To keep control on the number of evaluations
				return x;
			}
		}
		
		//_boundsOfLine == null means that the entire line is outside the box
		if(_boundsOfLine == null){
			return null;
		//Otherwise, if _boundsOfLine has a single intersecting point at one corner (i.e. in _boundsOfLine[0]) we return that point
		}else if (_boundsOfLine[1] == null){
			Vertex x = new Vertex(new Coordinates(_boundsOfLine[0]));
			x.setFitness(evaluator.evaluate(_boundsOfLine[0]));
			Termination.increment_evaluation();// To keep control on the number of evaluations
			return x;
		}
		
		//------------------------------------------------------------------
		// The second step is to optimize a line that is inscribed in the box (i.e. because none of the previous conditions has return something.)
		// That means that we have a line defined within the box.
		//------------------------------------------------------------------		
	
		double distance = initial_step_size;
		
		//initially the best so far point in the line is the init_p point
		// unless init_p is itself outside the bounds. If that is the case, we return the closest intersecting point
		Vertex bestsofar = returnVertexInBoundaries(init_p.getCoordinatesAsDouble(), _boundsOfLine, evaluator);
		
		// Some initializations
		double[] p1 = bestsofar.getCoordinates().getCoordinatesAsDouble();
		double[] p2 = any_p.getCoordinatesAsDouble();
		double[] v = new double[init_p.getDimensions()];

		
		//For the given No. of steps
		for(int i=0;i<steps;i++){
			
			//System.out.println(bestsofar.asString());
			
			// v is a vector in the line
			for(int j=0;j<v.length;j++){
				v[j] = p2[j] - p1[j];
			}
	 		
			//ref_hyperplane is an hyperplane that:
			// is orthogonal to v
			// contains p1
			double[] ref_hyperplane = getHyperplane(p1,v);
			
			//we find the two parallel hyperplanes to the ref_hyperplane at the given distance "distance"
			double[][] targetsinline = getTargets(distance, p2, p1, ref_hyperplane);
			
			
			//we calculate a new point in the outer target
			double[] p3 = Gradients.getIntersectionOfaVectorWithHyperplane(v, p2, targetsinline[1]);
			
			Vertex x = returnVertexInBoundaries(p3, _boundsOfLine, evaluator);
			
			if(x.better(bestsofar)){ // if p3 is better than the best so far
				bestsofar = x;
				p2 = p1.clone();
				p1 = p3.clone();
			}else{ // otherwise we look for a new point in the inner target
				p3 = Gradients.getIntersectionOfaVectorWithHyperplane(v, p2, targetsinline[0]);
				x = returnVertexInBoundaries(p3, _boundsOfLine, evaluator);
				if(x.better(bestsofar)){ 
					bestsofar = x;
					p2 = p1.clone();
					p1 = p3.clone();
				}else{
					p2 = p3.clone();
				}
			}
					
			distance /= 2.0;
			
		}
		
		return bestsofar;
	}
	
	
	
	/**
	 * 
	 * @param init_p is the point, defined in the line, from which we start the optimization.
	 * @param any_p is any point, defined in the line that we want to optimize.
	 * @param initial_step_size Depending on this parameter, the returned Vertex can be at a maximum distance of @param init_p of 2 times the initial_step_size 
	 * in either directions. This means that we cover 4 times the
	 * @param steps is the minimum number of steps that we perform in the search when every evaluation is successful (improves the search). Opposite, the 
	 * maximum number of evaluations is 2*steps if every step is unsuccessful (the search does not improve).
	 * @param evaluator is a IEvaluator with the fitness function F.
	 * @return A Vertex (let's call it P) with coordinates in the line defined by (init_p,any_p) with the following property:
	 * Assuming the line is unimodal in F, the distance of this point P to the optimal value in the interval [init_p-2*initial_step_size , init_p+2*initial_step_size] is of
	 * max. 1/(2^steps+1) * 4 * initial_step_size 
	 */
	public static Vertex optimizeline2(Coordinates init_p, Coordinates any_p, double initial_step_size, int steps, IEvaluator evaluator){
		
		//------------------------------------------------------------------
		// The first step is to compute the bounds of the line (intersecting points with the "box")
		//------------------------------------------------------------------
		double[][] _boundsOfLine;
		
		try {
			_boundsOfLine = Bounds.findBoundsOfLine(init_p.getCoordinatesAsDouble(), any_p.getCoordinatesAsDouble());
		} catch (Exception e) {// init_p and any_p are the same point or Matrix is singular (no line can be defined)
			//e.printStackTrace();
			if(Bounds.isCoordinateOutsideTheBounds(init_p.getCoordinatesAsDouble())){ //If the point is outside the bounds 
				return null;
			}else{//If is inside the bounds we return the point
				Vertex x = new Vertex(init_p);
				x.setFitness(evaluator.evaluate(init_p.getCoordinatesAsDouble()));
				Termination.increment_evaluation();// To keep control on the number of evaluations
				return x;
			}
		}
		
		//_boundsOfLine == null means that the entire line is outside the box
		if(_boundsOfLine == null){
			return null;
		//Otherwise, if _boundsOfLine has a single intersecting point at one corner (i.e. in _boundsOfLine[0]) we return that point
		}else if (_boundsOfLine[1] == null){
			Vertex x = new Vertex(new Coordinates(_boundsOfLine[0]));
			x.setFitness(evaluator.evaluate(_boundsOfLine[0]));
			Termination.increment_evaluation();// To keep control on the number of evaluations
			return x;
		}
		
		//------------------------------------------------------------------
		// The second step is to optimize a line that is inscribed in the box (i.e. because none of the previous conditions has return something.)
		// That means that we have a line defined within the box.
		//------------------------------------------------------------------		
	
		double distance = initial_step_size;
		
		//initially the best so far point in the line is the init_p point
		// unless init_p is itself outside the bounds. If that is the case, we return the closest intersecting point
		Vertex bestsofar = returnVertexInBoundaries(init_p.getCoordinatesAsDouble(), _boundsOfLine, evaluator);
		Vertex secondsofar = new Vertex((Coordinates)bestsofar.getCoordinates().clone());
		
		try {
			secondsofar.setFitness(bestsofar.getFitness());
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		// Some initializations
		double[] p1 = bestsofar.getCoordinates().getCoordinatesAsDouble();
		double[] p2 = any_p.getCoordinatesAsDouble();
		double[] v = new double[init_p.getDimensions()];

		
		//For the given No. of steps
		for(int i=0;i<steps;i++){
			
			//System.out.println(bestsofar.asString());
			
			// v is a vector in the line
			for(int j=0;j<v.length;j++){
				v[j] = p2[j] - p1[j];
			}
	 		
			//ref_hyperplane is an hyperplane that:
			// is orthogonal to v
			// contains p1
			double[] ref_hyperplane = getHyperplane(p1,v);
			
			//we find the two parallel hyperplanes to the ref_hyperplane at the given distance "distance"
			double[][] targets = getTargets(distance, p2, p1, ref_hyperplane);
			
			
			//we calculate a new point in the outer target
			double[] p3 = Gradients.getIntersectionOfaVectorWithHyperplane(v, p2, targets[1]);
			
			Vertex x = returnVertexInBoundaries(p3, _boundsOfLine, evaluator);
			
			if(x.better(bestsofar)){ // if p3 is better than the best so far
				secondsofar = bestsofar;
				bestsofar = x;
				p2 = p1.clone();
				p1 = p3.clone();
			}else{ // otherwise we look for a new point in the inner target
				p3 = Gradients.getIntersectionOfaVectorWithHyperplane(v, p2, targets[0]);
				x = returnVertexInBoundaries(p3, _boundsOfLine, evaluator);
				if(x.better(bestsofar)){
					secondsofar = bestsofar;
					bestsofar = x;
					p2 = p1.clone();
					p1 = p3.clone();
				}else{
					p2 = p3.clone();
				}
			}
					
			distance /= 2.0;
			
		}
		
		return secondsofar;
	}
	
	private static Vertex returnVertexInBoundaries(double[] coordinate, double[][] boundsofline, IEvaluator evaluator){
		if(Bounds.isCoordinateOutsideTheBounds(coordinate)){
			double[] x = Bounds.closestIntersectingBoundToAPoint(coordinate, boundsofline);
			Vertex P = new Vertex(new Coordinates(x));
			P.setFitness(evaluator.evaluate(x));
			Termination.increment_evaluation();// To keep control on the number of evaluations
			return P;
		}else{
			Vertex P = new Vertex(new Coordinates(coordinate));
			P.setFitness(evaluator.evaluate(coordinate));
			Termination.increment_evaluation();// To keep control on the number of evaluations
			return P;
		}
	}
	
	/**
	 * n is a normal vector to the hyperplane, p1 belongs to the hyperplane
	 * @param p1 is a point defined in the hyperplane
	 * @param n is a normal vector of the hyperplane
	 * @return the coefficients of an hyperplane
	 */
	private static double[] getHyperplane(double[] p1, double[] n){
		
		double[] coefficients = new double[p1.length+1];
		
		//Normal vector n (p1p2)
		//We get here A, B and C for the equation of the hyperplane:
		//Ax + By + Cz - D = 0
		for(int i=0;i<p1.length;i++) coefficients[i] = n[i];
		
		
		//-D = -Axo -Byo -Czo
		//where (xo,yo,zo) = p1
		double D = 0;
		for(int i=0;i<p1.length;i++){
			D -= coefficients[i] * p1[i];
		}	
		coefficients[Configuration.N] = D;
			
		return coefficients;
	}
	
	/**
	 * Get both target hyperplanes at a given distance.
	 * The inner target is at position 0 and is the closest to ref_point.
	 * The outside target is at position 1
	 * @param distance This is the distance we want from the @param ref_hyperplane to the targets hyperplanes
	 * @param ref_point The ref_point is a point outside the @param ref_hyperplane. We use it to diferenciate between the inner and the outer target
	 * @param point_at_ref_hyp This point belongs to the reference hyperplane given by  @param ref_hyperplane
	 * @param ref_hyperplane The reference hyperplane
	 * @return the two targets hyperplanes. The inner target at position 0, and the outer target at position 1.
	 */
	private static double[][] getTargets(double distance,double[] ref_point, double[] point_at_ref_hyp, double[] ref_hyperplane){
		
		// Direction coefficients for the "targets" are equal to those of the "ref_hyperplane", i.e. they are parallel hyperplanes
		double[][] targets = new double[2][Configuration.N+1];
		System.arraycopy(ref_hyperplane, 0, targets[0], 0, Configuration.N);
		System.arraycopy(ref_hyperplane, 0, targets[1], 0, Configuration.N);
		
//We need to calculate the remaining coefficient, i.e. D in Ax+By+Cz+D=0
// Since the ref_hyperplane has two parallel hyperplanes at the same distance from the ref_hyperplane...
// ... the two different Ds stand for the inner and outside target

		
		//Calculating D for the hyperplane, parallel to the ref_hyperplane, that contains the ref_point
		double Dref = 0;
		for(int i=0;i<Configuration.N;i++){
			Dref -= ref_hyperplane[i] * ref_point[i]; 
		}
		
		//Calculating the Ds for the two hyperplanes at the same distance of the ref_hyperplane
		// distance = |Axo + Byo + Czo + D| / sqrt(A^2 + B^2 + C^2)
		// where xo,yo,zo is the point_at_ref_hyp point at the ref_hyperplane
		
		// sqrt(A^2 + B^2 + C^2)
		double sqrt=0;
		for(int i=0;i<Configuration.N;i++){
			sqrt += Math.pow(ref_hyperplane[i],2) ; 
		}
		sqrt = Math.sqrt(sqrt);
		
		// distance * sqrt(A^2 + B^2 + C^2)
		double distanceXsqrt = distance * sqrt;
		
		//  Axo + Byo + Czo is the point_at_ref_hyp point
		double aixi = 0;
		for(int i=0;i<Configuration.N;i++){
			aixi -= ref_hyperplane[i] * point_at_ref_hyp[i] ; 
		}
		
		// D = Axo + Byo + Czo (+-) distance * sqrt(A^2 + B^2 + C^2)
		double D1 = aixi + distanceXsqrt;
		double D2 = aixi - distanceXsqrt;
		
		// Now we have, 4 parallel hyperplanes all with the same coefficient A B C
		// - The ref_hyperplane with D
		// - The parallel hyperplane passing by the ref_point with Dref
		// - Two parallel hyperplanes to the ref_hyperplane at the given distance from the ref_hyperplane
		
		//We have to order the target hyperplanes, the inner (0) is the closer to the ref_point
		// while the outer (1) is the furthest from the ref_point
		if(Dref > ref_hyperplane[Configuration.N]){
			if(D1 < ref_hyperplane[Configuration.N]){
				targets[0][Configuration.N] = D2;
				targets[1][Configuration.N] = D1;
			}else{
				targets[0][Configuration.N] = D1;
				targets[1][Configuration.N] = D2;
			}
		}else{
			if(D1 > ref_hyperplane[Configuration.N]){
				targets[0][Configuration.N] = D2;
				targets[1][Configuration.N] = D1;
			}else{
				targets[0][Configuration.N] = D1;
				targets[1][Configuration.N] = D2;
			}
		}
				
		return targets;
	}
	
	public static double norm(double[] v1,double[] v2){
		double sum = 0;
		for(int i=0;i<v1.length;i++){
			sum += Math.pow(v1[i]-v2[i],2);
		}
		return Math.sqrt(sum);
	}
	
	public static void main(String[] args) {
		//Setting up properties
				LoadProperties lp = new LoadProperties(args);
				Configuration.setConfiguration(lp);	
				Configuration.F=2;
				Configuration.minimization = true;
				Configuration.N=2;
				Configuration.upper_bound= 100;
				Configuration.lower_bound= -100;
				Configuration.setBounds();
					
				
				//Testing the structure of the first simplex
				Cec2014 bench = new Cec2014();
				
				double[] p1 = {-51.7,43.42};
				double[] p2 = {-51.7,0};
				optimizeline(new Coordinates(p1), new Coordinates(p2), 42, 20, bench);
				
				
	}
}
